/*********************************************************************
* File: Navigation.cpp
*
* Implements basic and free navigation as well as collision handling. 
*
* \author Instituto Superior Tecnico de Lisboa, Portugal
* \author Technical University of Lisbon, Portugal
* 
* \author Agentes Autonomos e Sistemas Multiagente
* \author Autonomous Agents and Multiagent Systems
* 
* \version	1.0
* \date		2006/2007
*********************************************************************/
#include "Navigation.h"
#include "../../util/mathLibrary.h"
#include "../../util/util.h"
#include "Senses.h"
#include "../Agent.h"
using namespace agents;

#pragma warning( disable : 4482 )  // Disable warning messages

const Vector IFreeNavigation::destinationUndefined(-999999.9999,-999999.9999,-999999.9999);

/**
* Class constructor
*
* \param[in] the agent
**/
Navigation::Navigation(Agent &owner)
:_owner(owner)
{
	setCollisionHandler(this);

	_frontSpeed = 1;
	_sideSpeed = 1;
	_rotationSpeed = 1;
}

/**
* \brief Class destructor.
**/
Navigation::~Navigation(){}


/**
* \brief Gets the agent's position.
*
* \see agents::interfaces::INavigation::getPosition
**/
Vector Navigation::getPosition()
{
	return _owner.pev->origin;
}
/**
* \brief Gets the agent's moving angles.
*
* \see agents::interfaces::INavigation::getAngles
**/
Vector Navigation::getAngles()
{
	/*return _owner.pev->angles;*/
	return _moveAngles;
}
/**
* \brief Sets the agent's moving angles.
**/
void Navigation::setAngles(Vector angles)
{
	/*_owner.pev->angles = angles;*/
	_moveAngles = angles;
}
/**
* \brief Moves the agent forward.
*
* \see agents::interfaces::INavigation::moveForward
**/
void Navigation::moveForward()
{
	_owner.pev->button |= IN_FORWARD;
}
/**
* \brief Moves the agent backwards.
*
* \see agents::interfaces::INavigation::moveBackwards
**/
void Navigation::moveBackwards()
{
	_owner.pev->button |= IN_BACK;
}
/**
* \brief Moves the agent left.
*
* \see agents::interfaces::INavigation::moveLeft
**/
void Navigation::moveLeft()
{
	_owner.pev->button |= IN_MOVELEFT;
}
/**
* \brief Moves the agent right.
*
* \see agents::interfaces::INavigation::moveRight
**/
void Navigation::moveRight()
{
	_owner.pev->button |= IN_MOVERIGHT;
}
/**
* \brief Rotates the agent left.
*
* \see agents::interfaces::INavigation::rotateLeft
**/
void Navigation::rotateLeft()
{
	Vector angles = getAngles();
	angles.y += gpGlobals->frametime * _rotationSpeed * MAXIMUM_MOTION_ROTATION_SPEED;
	setAngles(angles);
}
/**
* \brief Rotates the agent right.
*
* \see agents::interfaces::INavigation::rotateRight
**/
void Navigation::rotateRight()
{
	Vector angles = getAngles();
	angles.y -= gpGlobals->frametime * _rotationSpeed * MAXIMUM_MOTION_ROTATION_SPEED;
	setAngles(angles);
}
/**
* \brief Rotates the agent up.
**/
void Navigation::rotateUp()
{
	Vector angles = getAngles();
	angles.x += gpGlobals->frametime * _rotationSpeed * MAXIMUM_MOTION_ROTATION_SPEED;
	setAngles(angles);
}
/**
* \brief Rotates the agent down.
**/
void Navigation::rotateDown()
{
	Vector angles = getAngles();
	angles.x -= gpGlobals->frametime * _rotationSpeed * MAXIMUM_MOTION_ROTATION_SPEED;
	setAngles(angles);
}
/**
* \brief Makes the agent jump.
*
* \see agents::interfaces::INavigation::jump
**/
void Navigation::jump()
{
	_owner.pev->button |= IN_JUMP;
}
/**
* \brief Makes the agent duck.
*
* \see agents::interfaces::INavigation::duck
**/
void Navigation::duck()
{
	_owner.pev->button |= IN_DUCK;
}
/**
* \brief Gets the front speed.
*
* \see agents::interfaces::INavigationSpeedManager::getFrontSpeed
**/
float Navigation::getFrontSpeed()
{
	return _frontSpeed;
}
/**
* \brief Sets the front speed.
*
* \see agents::interfaces::INavigationSpeedManager::setFrontSpeed
**/
void Navigation::setFrontSpeed(float speed)
{
	if(speed > 1)
		speed = 1;
	else if(speed < 0)
		speed = 1;
	_frontSpeed = speed;
}
/**
* \brief Gets the side speed.
*
* \see agents::interfaces::INavigationSpeedManager::getSideSpeed
**/
float Navigation::getSideSpeed()
{
	return _sideSpeed;
}
/**
* \brief Sets the side speed.
*
* \see agents::interfaces::INavigationSpeedManager::setSideSpeed
**/
void Navigation::setSideSpeed(float speed)
{
	if(speed > 1)
		speed = 1;
	else if(speed < 0)
		speed = 1;
	_sideSpeed = speed;
}
/**
* \brief Gets the rotation speed.
*
* \see agents::interfaces::INavigationSpeedManager::getRotationSpeed
**/
float Navigation::getRotationSpeed()
{
	return _rotationSpeed;
}
/**
* \brief Sets the rotation speed.
*
* \see agents::interfaces::INavigationSpeedManager::setRotationSpeed
**/
void Navigation::setRotationSpeed(float speed)
{
	if(speed > 1)
		speed = 1;
	else if(speed < 0)
		speed = 1;
	_rotationSpeed = speed;
}
/**
* \brief Gets the effective front speed.
*
* The effective front speed is 0 if neither the moveForward or moveBackwards
* actuators has been invoked; otherwise, is the side speed. 
**/
float Navigation::getEffectiveFrontSpeed()
{
	if(_owner.pev->button & IN_FORWARD)
		return (_frontSpeed * _rampFrontSpeed * _owner.pev->maxspeed);
	else if(_owner.pev->button & IN_BACK)
		return -(_frontSpeed * _rampFrontSpeed * _owner.pev->maxspeed);
	else
		return 0;
}
/**
* \brief Gets the effective side speed.
* 
* The effective side speed is 0 if neither the moveRight or moveLeft 
* actuators has been invoked; otherwise, is the side speed. 
**/
float Navigation::getEffectiveSideSpeed()
{
	if(_owner.pev->button & IN_MOVERIGHT)
		return (_sideSpeed * _owner.pev->maxspeed); 
	else if(_owner.pev->button & IN_MOVELEFT)
		return -(_sideSpeed * _owner.pev->maxspeed); 
	else
		return 0;
}
/**
* \brief Moves the agent towards a position in the world.
* 
* \see agents::interfaces::IFreeNavigation::moveToDestination
**/
void Navigation::moveToDestination(Vector position)
{
	_destination = position;
}
/**
* \brief Stops the agent.
*
* Discards current the current destination.
*
* \see agents::interfaces::IFreeNavigation::stop
**/
void Navigation::stop()
{
	_destination = destinationUndefined;
}
/**
* \brief Whether the agent has reached the intended destination.
*
* \see agents::interfaces::IFreeNavigation::reachedDestination
**/
bool Navigation::reachedDestination()
{
	return util::math::compareVectors(_destination, destinationUndefined, 0);
}
/**
* Should be invoked when a new round starts
**/
void Navigation::newRound()
{
	_destination = destinationUndefined;//g_vecZero);

	_absPreviousSpeed = 0.0;  // fake "paused" since agent is NOT stuck
	_previousPosition = Vector(9999.0, 9999.0, 9999.0);
	_nextCollisionDetectionTime = gpGlobals->time;
	_isColliding = false;
	_isStuck = false;
	_collisionStartTime = -1;
	_cutoffDistance = 15.0;

	resetCollisionState();
}
/**
* Prepares the navigation cycle. Should be invoked at the start of each cycle
**/
void Navigation::startCycle()
{
	_ladderMovementDirection = Navigation::LadderMovementDirections::none;
}

/**
* Ends the navigation cycle. Should be invoked at the end of each cycle
**/
void Navigation::endCycle()
{
	if(!reachedDestination())
	{
		Vector toTargetVector = _destination - getPosition();
		Vector moveAngles = util::math::vectorToAngles(toTargetVector);
		util::math::clampAngles(moveAngles);
		moveAngles.x *= -1.0; // Invert for engine
		setAngles(moveAngles);
		_rampFrontSpeed = 1.0;

		float distanceToTarget = toTargetVector.Length();
		float rampDistance = _cutoffDistance * 2;
		if(distanceToTarget > _cutoffDistance)
		{
			if (distanceToTarget < rampDistance)
				_rampFrontSpeed = distanceToTarget / rampDistance;
			moveForward(); // Move towards destination
		}
		else
			Navigation::stop();
	}

	if (_owner.isOnLadder()) 
		handleLadderNavigation();
	/*else if (_owner.isInWater()) 
		handleWaterNavigation();*/
	
	if((_collisionHandler != 0) && (_collisionHandler->detectCollision()))
		_collisionHandler->handleCollision();
	_absPreviousSpeed = fabs(getEffectiveFrontSpeed()); // For collision handling algorithm

	util::math::clampAngles(_owner.pev->angles);
	util::math::clampAngles(_owner.pev->v_angle);
	util::math::clampAngles(_moveAngles);
}

/**
* Handles ladder navigation
**/
void Navigation::handleLadderNavigation()
{
	moveForward();

	// Get speed multiply factor by dividing target FPS by real FPS
	float fSpeed = 480 * g_frameDuration;

	if (_ladderMovementDirection == LadderMovementDirections::down)
	{
		Vector moveAngles = getAngles();
		moveAngles.x = 60; // move down
		setAngles(moveAngles);

		float delta = util::math::angleDifference(_owner.pev->angles.x, -20);

		if (fabs(delta) < fSpeed)
			_owner.pev->angles.x = -20;
		else
		{
			if (delta > 0)
				_owner.pev->angles.x += fSpeed;
			else
				_owner.pev->angles.x -= fSpeed;
		}
	}
	else if (_ladderMovementDirection == LadderMovementDirections::up)
	{
		Vector moveAngles = getAngles();
		moveAngles.x = -60; // move up
		setAngles(moveAngles);

		float delta = util::math::angleDifference(_owner.pev->angles.x, 20);

		if (fabs(delta) < fSpeed)
			_owner.pev->angles.x = 20;
		else
		{
			if (delta > 0)
				_owner.pev->angles.x += fSpeed;
			else
				_owner.pev->angles.x -= fSpeed;
		}
	}

	util::math::clampAngles(_owner.pev->angles);
}

/**
* Handles water navigation
**/
void Navigation::handleWaterNavigation(){
	Vector vecViewPos = _owner.eyePosition();
		
	// Check if we need to go forward or back
	int iAngle = _owner.senses->getAngleInFieldOfView(_destination - vecViewPos);

	// Press the correct buttons
	if (iAngle > 90)
		_owner.pev->button |= IN_BACK;
	else
		_owner.pev->button |= IN_FORWARD;

	if (getAngles().x > 60.0)
		duck();
	else if (getAngles().x < -60.0)
		jump();
}
/**
* \brief Resets the collision handling state.
**/
void Navigation::resetCollisionState()
{
	_collisionProbeTime = 0.0;
	_collisionState = CollisionStates::notDecided;
	_collisionStateIndex = 0;

	// Bug 05 - Reset collision state - Start
	_isStuck = false;
	_isColliding = false;
	// Bug 05 - End
}
/**
* \brief The default collision detection algorithm.
* 
* This code is adapted from YapB Agent (http://yapb.bots-united.com/).
* 
* \see agents::interfaces::ICollisionHandler::detectCollision
**/
bool Navigation::detectCollision()
{
	if(reachedDestination())
	{
		// Bug 05 - Reset collision state - Start
		resetCollisionState();
		// Bug 05 - End
	}

	if ((getEffectiveFrontSpeed() < 10) && (getEffectiveSideSpeed() < 10))
		return false;

	Vector diff;        // vector from previous to current location
	float movedDistance; // length of diff vector (distance agent moved)

	if (_nextCollisionDetectionTime <= gpGlobals->time)
	{
		// see how far the agent has moved since the previous position...
		diff = _previousPosition - getPosition();
		movedDistance = diff.Length();

		// save current position as previous
		_previousPosition = getPosition();
		_nextCollisionDetectionTime = gpGlobals->time + 0.2;
	}
	else
		movedDistance = MINIMUM_COLLISION_FREE_DISTANCE;

	if ((movedDistance < MINIMUM_COLLISION_FREE_DISTANCE) && (_absPreviousSpeed > 20.0)  && (_rampFrontSpeed >= 0.5)) // Did we move enough?
	{
		_nextCollisionDetectionTime = gpGlobals->time;
		
		if(!_isColliding)
		{
			_isColliding = true;
			_collisionStartTime = gpGlobals->time;
			_collisionPosition = getPosition();
		}

		if(_isColliding && ((gpGlobals->time - _collisionStartTime) > 5.0))
			_isStuck = true;

		return true;
	}
	
	return false;
}
/**
* \brief The default collision resolution algorithm.
*
* This code is adapted from YapB Agent (http://yapb.bots-united.com/).
* 
* \see agents::interfaces::ICollisionHandler::handleCollision
**/
void Navigation::handleCollision()
{
	TraceResult tr;
	Vector v_dest;
	Vector v_src;
	Vector v_direction;

	// Calculate 2 direction Vectors, 1 without the up/down component
	v_direction = _destination - (getPosition() + _owner.pev->velocity * g_frameDuration);
	Vector vecDirectionNormal = v_direction.Normalize();
	Vector vecDirection = vecDirectionNormal;
	vecDirectionNormal.z = 0.0;

	// Not yet decided what to do?
	if (_collisionState == CollisionStates::notDecided)
	{
		char cBits;

		if (_owner.isOnLadder())
			cBits = CollisionProbingStates::strafe;
		else if (_owner.isInWater())
			cBits = (CollisionProbingStates::jump | CollisionProbingStates::strafe);
		else
			cBits = (CollisionProbingStates::jump | CollisionProbingStates::strafe | CollisionProbingStates::duck);

		// Collision check allowed if not flying through the air
		if ((_owner.pev->flags & FL_ONGROUND) || _owner.isOnLadder() || _owner.isInWater())
		{
			char cState[10];
			int i = 0;

			// First 5 Entries hold the possible Collision States
			cState[i++] = CollisionStates::jump;
			cState[i++] = CollisionStates::duck;
			cState[i++] = CollisionStates::jumpAndDuck;
			cState[i++] = CollisionStates::strafeLeft;
			cState[i++] = CollisionStates::strafeRight;

			// Now weight all possible states
			if (cBits & CollisionProbingStates::jump)
			{
				cState[i] = 0;
				if (_owner.canJumpUp(vecDirectionNormal))
					cState[i] += 10;
				if (_destination.z >= getPosition().z + 18.0)
					cState[i] += 5;
				if (pointIsReachable(_destination))
				{
					util::math::makeVectors(getAngles());
					v_src = _owner.eyePosition();
					v_src = v_src + gpGlobals->v_right * 15;
					util::traceline( v_src, _destination, ignore_monsters, ignore_glass, _owner.getEdict(), &tr);
					if (tr.flFraction >= 1.0)
					{
						v_src = _owner.eyePosition();
						v_src = v_src - gpGlobals->v_right * 15;
						util::traceline( v_src, _destination, ignore_monsters, ignore_glass, _owner.getEdict(), &tr);
						if (tr.flFraction >= 1.0)
							cState[i] += 5;
					}
				}
				if (_owner.pev->flags & FL_DUCKING)
					v_src = getPosition();
				else
					v_src = getPosition() + Vector(0, 0, -17);
				v_dest = v_src + vecDirectionNormal * 30;
				util::traceline( v_src, v_dest, ignore_monsters, ignore_glass, _owner.getEdict(), &tr);
				if (tr.flFraction != 1.0)
					cState[i] += 10;
			}
			else
				cState[i] = 0;
			i++;

			if (cBits & CollisionProbingStates::duck)
			{
				cState[i] = 0;
				if (_owner.canDuckUnder(vecDirectionNormal))
					cState[i] += 10;
				if (_destination.z + 36.0 <= getPosition().z && pointIsReachable(_destination))
					cState[i] += 5;
			}
			else
				cState[i] = 0;
			i++;

			if (cBits & CollisionProbingStates::strafe)
			{
				cState[i] = 0;
				cState[i + 1] = 0;

				Vector2D vec2DirToPoint;
				Vector2D vec2RightSide;

				// to start strafing, we have to first figure out if the target is on the left side or right side
				util::math::makeVectors(getAngles());

				vec2DirToPoint = (getPosition() - _destination).Make2D().Normalize();
				vec2RightSide = gpGlobals->v_right.Make2D().Normalize();
				bool bDirRight = false;
				bool bDirLeft = false;
				bool bBlockedLeft = false;
				bool bBlockedRight = false;

				if ( DotProduct ( vec2DirToPoint, vec2RightSide ) > 0 )
					bDirRight = true;
				else
					bDirLeft = true;

				//if (_moveSpeed > 0)
				if (getEffectiveFrontSpeed() > 0)
					vecDirection = gpGlobals->v_forward;
				/*else
					vecDirection = -gpGlobals->v_forward;*/

				// Now check which side is blocked
				v_src = getPosition() + gpGlobals->v_right * 32;
				v_dest = v_src + vecDirection * 32;
				util::traceHull(v_src, v_dest, ignore_monsters, head_hull, _owner.getEdict(), &tr);
				if (tr.flFraction != 1.0)
					bBlockedRight = true;

				v_src = getPosition() - gpGlobals->v_right * 32;
				v_dest = v_src + vecDirection * 32;
				util::traceHull(v_src, v_dest, ignore_monsters, head_hull, _owner.getEdict(), &tr);
				if (tr.flFraction != 1.0)
					bBlockedLeft = true;

				if (bDirLeft)
					cState[i] += 5;
				else
					cState[i] -= 5;

				if (bBlockedLeft)
					cState[i] -= 5;

				i++;

				if (bDirRight)
					cState[i] += 5;
				else
					cState[i] -= 5;

				if (bBlockedRight)
					cState[i] -= 5;
			}
			else
			{
				cState[i] = 0;
				i++;
				cState[i] = 0;
			}

			// Weighted all possible moves, now sort them to start with most probable
			char cTemp;
			bool bSorting;
			do
			{
				bSorting = false;
				for (i = 0; i < 4; i++)
				{
					if (cState[i + 5] < cState[i + 6])
					{
						cTemp = cState[i];
						cState[i] = cState[i + 1];
						cState[i + 1] = cTemp;
						cTemp = cState[i + 5];
						cState[i + 5] = cState[i + 6];
						cState[i + 6] = cTemp;
						bSorting = true;
					}
				}
			} while(bSorting);

			for (i = 0; i < 5; i++)
				_collisionMoves[i] = cState[i];
			_collisionProbeTime = gpGlobals->time + 0.5;
			_collisionState = CollisionStates::probing;
			_collisionStateIndex = 0;
		}
	}

	if(_collisionState == CollisionStates::probing)
	{
		if (_collisionProbeTime < gpGlobals->time)
		{
			_collisionStateIndex++;
			_collisionProbeTime = gpGlobals->time + 0.5;
			if (_collisionStateIndex > 5)
				resetCollisionState();
		}

		if (_collisionStateIndex <= 5)
		{
			switch (_collisionMoves[_collisionStateIndex])
			{
			case CollisionStates::jump:
				if ((_owner.pev->flags & FL_ONGROUND) || _owner.isInWater())
					jump();
				break;

			case CollisionStates::duck:
				if ((_owner.pev->flags & FL_ONGROUND) || _owner.isInWater())
					duck();
				break;

			case CollisionStates::jumpAndDuck:
				if ((_owner.pev->flags & FL_ONGROUND) || _owner.isInWater())
					jump();
				if ((_owner.pev->flags & FL_ONGROUND) || _owner.isInWater())
					duck();
				break;

			case CollisionStates::strafeLeft:
				moveLeft();
				break;

			case CollisionStates::strafeRight:
				moveRight();
				break;
			}
		}
	}
}
/**
* Whether an entity is visible
*
* \param[in] vecDest the entity origin
**/
bool Navigation::pointIsReachable( Vector point )
{
	TraceResult tr;

	// trace a line from bot's eyes to destination...
	util::traceline(_owner.eyePosition(), point, ignore_monsters, _owner.getEdict(), &tr);

	// check if line of sight to object is not blocked (i.e. visible)
	return (tr.flFraction >= 1.0);
}


// Update 1: Jump/Duck sensors : Start
/**
* \brief Whether the agent is jumping.
**/
bool Navigation::isJumping()
{
	return !(_owner.pev->flags & FL_ONGROUND);
}
/**
* \brief Whether the agent is ducking.
**/
bool Navigation::isDucking()
{
	return (_owner.pev->flags & FL_DUCKING);
}
// Update 1: End




